#include "modules/MonitorModule/PostionStuckMonitor.h"
#include "modules/CtrlModule/PassivePlanner/PostionStuckPlanner.h"
#include "data_struct/base/DataSet.h"
#include "modules/CtrlModule/MarkPosePubModel.h"

namespace  behavior_controller
{
    #define MAX_POWER 200
    #define MAX_CURRENT 2
    #define SPEED_DIFF 0.2
    PostionStuckMonitor postion_stuck_monitor;
    void PostionStuckMonitor::run()
    {
        if(postion_stuck_planner.finished)
        {
            bool is_stuck=false;
            if((DataSet::motor_data.power_wheel_left()>=MAX_POWER|| 
                DataSet::motor_data.power_wheel_right()>=MAX_POWER)              && 
                (fabs(speed_linear_x()-DataSet::odom_data.vel_x())>SPEED_DIFF||
                 fabs(speed_angle_z()-DataSet::odom_data.vel_z()>SPEED_DIFF))     &&
                DataSet::motor_data.overcurrent_flag()>=MAX_CURRENT)
            {
                is_stuck=true;
            }
            if(is_stuck)
            {
                DataSet::ctrl_state = POSITION_STUCK_PLAN;
                postion_stuck_planner.finished=false;
                PointF robot_pose_tmp(DataSet::robot_pose.x(),DataSet::robot_pose.y());
                std::vector<PointF> pose_array;
                pose_array.emplace_back(robot_pose_tmp);
                mark_pose_pub_mod.MarkPosePub(pose_array);
                std::cout<< "postion_stuck  bad !"<< std::endl;
            }
        }
    }
}